using System;
using System.Collections.Generic;
using UnityEngine;

namespace Gj.Pathfinding
{
    public class MultiJPS : Map
    {
        private readonly List<MultiArea> _areas;

        private readonly Dijkstra _dijkstra;
        private readonly JPSInterface _instance;
        private readonly Queue<JPS> _jpsQueue;
        private readonly Queue<List<KeyValuePair<MultiNode, MultiArea>>> _multiKVQueue;
        private readonly Queue<List<Node>> _multiQueue;
        private readonly Dictionary<int, Dictionary<int, int>> _navigable;
        private readonly Dictionary<Vector2Int, MultiNode> _nodes;

        private readonly Queue<List<Vector2Int>> _resultQueue;

        private Dictionary<Vector2Int, MultiNode>.Enumerator e;
        private bool isE;

        public MultiJPS(JPSInterface instance, Dictionary<int, Dictionary<int, int>> navigable)
        {
            _navigable = navigable;
            _instance = instance;
            _areas = new List<MultiArea>();
            _nodes = new Dictionary<Vector2Int, MultiNode>();
            _resultQueue = new Queue<List<Vector2Int>>();
            _jpsQueue = new Queue<JPS>();
            _multiQueue = new Queue<List<Node>>();
            _multiKVQueue = new Queue<List<KeyValuePair<MultiNode, MultiArea>>>();
            _dijkstra = new Dijkstra();
        }

        public bool MoveNext()
        {
            if (!isE)
            {
                isE = true;
                e = _nodes.GetEnumerator();
            }

            return e.MoveNext();
        }

        public Node Current()
        {
            return e.Current.Value;
        }

        public void Dispose()
        {
            e.Dispose();
            isE = false;
        }

        private int GetStatus(Vector2Int point)
        {
            Dictionary<int, int> yy;
            if (!_navigable.TryGetValue(point.x, out yy))
            {
                yy = new Dictionary<int, int>();
                _navigable.Add(point.x, yy);
            }

            if (yy.ContainsKey(point.y))
                return yy[point.y];
            return 0;
        }

        private void SetStatus(Vector2Int point, int status)
        {
            Dictionary<int, int> yy;
            if (!_navigable.TryGetValue(point.x, out yy))
            {
                yy = new Dictionary<int, int>();
                _navigable.Add(point.x, yy);
            }

            if (yy.ContainsKey(point.y))
                yy[point.y] = status;
            else
                yy.Add(point.y, status);
        }

        public MultiArea AddArea(Vector2Int[] Polygon, Vector2Int[] Points)
        {
            var area = new MultiArea(Polygon, _navigable);

            _areas.Add(area);

            for (var i = 0; i < Points.Length; i++)
            {
                MultiNode node;
                if (!_nodes.TryGetValue(Points[i], out node))
                {
                    node = new MultiNode();
                    node.Areas = new MultiArea[2];
                    node._Point = Points[i];
                    node._Status = GetStatus(node._Point);
                    _nodes[node._Point] = node;
                }

                area._nodes.Add(node);
                node.Bind(area);
                node.ConnectDuplex(area._nodes);
            }

            return area;
        }

        public void UpdateNode(Vector2Int point, int status)
        {
            SetStatus(point, status);
            MultiNode node;
            if (_nodes.TryGetValue(point, out node)) node._Status = status;
            for (var i = 0; i < _areas.Count; i++)
                if (_areas[i].Contains(point))
                {
                    _areas[i].Tailor(ref point);
                    _areas[i]._grid.Update(point, status);
                    break;
                }
        }

        public bool FindPath(Vector2Int start, Vector2Int end, int mask, out Vector2Int[] rr)
        {
            var result = _resultQueue.Count > 0 ? _resultQueue.Dequeue() : new List<Vector2Int>();

            var f = 0;
            MultiArea startArea = null;
            MultiArea endArea = null;
            var r = false;
            for (var i = 0; i < _areas.Count; i++)
            {
                if ((f & 1) == 0 && _areas[i].Contains(start))
                {
                    startArea = _areas[i];
                    f |= 1;
                }

                if ((f & 2) == 0 && _areas[i].Contains(end))
                {
                    endArea = _areas[i];
                    f |= 2;
                }

                if (f == 3) break;
            }

            if (f != 3 || startArea == null || endArea == null)
            {
                rr = default;
                return r;
            }

            result.Clear();
            if (startArea == endArea)
                r = SimpleFindPath(startArea, start, end, mask, ref result);
            else
                r = MultiFindPath(startArea, endArea, start, end, mask, ref result);

            rr = result.ToArray();
            _resultQueue.Enqueue(result);
            return r;
        }

        private bool SimpleFindPath(MultiArea area, Vector2Int start, Vector2Int end, int mask,
            ref List<Vector2Int> result)
        {
            var jps = _jpsQueue.Count > 0 ? _jpsQueue.Dequeue() : new JPS(_instance);

            area.Tailor(ref start);
            area.Tailor(ref end);
            var r = jps.FindPath(area._grid, start, end, mask, ref result);
            if (r) area.Restore(ref result);
            _jpsQueue.Enqueue(jps);
            return r;
        }

        private bool MultiFindPath(MultiArea startArea, MultiArea endArea, Vector2Int start, Vector2Int end, int mask,
            ref List<Vector2Int> result)
        {
            var rr = _multiKVQueue.Count > 0 ? _multiKVQueue.Dequeue() : new List<KeyValuePair<MultiNode, MultiArea>>();
            var r = NodePath(startArea, endArea, start, end, mask, ref rr);
            if (r)
            {
                var rrr = _resultQueue.Count > 0 ? _resultQueue.Dequeue() : new List<Vector2Int>();
                for (var i = 0; i < rr.Count; i++)
                {
                    rrr.Clear();
                    var kv = rr[i];
                    SimpleFindPath(kv.Value, start, kv.Key._Point, mask, ref rrr);
                    start = kv.Key._Point;
                    result.AddRange(rrr);
                }

                _resultQueue.Enqueue(rrr);
            }

            _multiKVQueue.Enqueue(rr);
            return r;
        }

        public bool NodePath(MultiArea startArea, MultiArea endArea, Vector2Int start, Vector2Int end, int mask,
            ref List<KeyValuePair<MultiNode, MultiArea>> result)
        {
            var rr = _multiQueue.Count > 0 ? _multiQueue.Dequeue() : new List<Node>();
            rr.Clear();

            var startNode = new MultiNode();
            startNode.Areas = new MultiArea[2];
            startNode._Status = GetStatus(start);
            startNode._Point = start;
            startNode.Bind(startArea);
            startNode.ConnectSimplex(startArea._nodes);

            var endNode = new MultiNode();
            endNode.Areas = new MultiArea[2];
            startNode._Status = GetStatus(end);
            endNode._Point = end;
            endNode.Bind(endArea);
            endNode.ConnectSimplex(endArea._nodes);

            var cost = _dijkstra.ToDijkstra(startNode, endNode, mask, ref rr, _dijkstra.AStarCost);
            if (rr.Count > 0)
                for (var i = 1; i < rr.Count; i++)
                {
                    var node = rr[i] as MultiNode;
                    var kv = new KeyValuePair<MultiNode, MultiArea>(node, startArea);
                    result.Add(kv);
                    startArea = node.Areas[0] == startArea ? node.Areas[1] : node.Areas[0];
                }

            _multiQueue.Enqueue(rr);
            return cost > 0;
        }
    }

    public class MultiNode : LinkNode
    {
        internal MultiArea[] Areas;

        public void Bind(MultiArea area)
        {
            if (Areas[0] == null)
                Areas[0] = area;
            else if (Areas[1] == null)
                Areas[1] = area;
            else
                throw new Exception("Node only two area connect");
        }
    }

    public class MultiArea
    {
        internal JPSGrid _grid;
        internal List<LinkNode> _nodes;
        internal Vector2Int[] Polygon;
        internal Vector2Int Start;

        public MultiArea(Vector2Int[] Polygon, Dictionary<int, Dictionary<int, int>> navigable)
        {
            this.Polygon = Polygon;
            var minX = int.MaxValue;
            var minY = int.MaxValue;
            var maxX = int.MinValue;
            var maxY = int.MinValue;

            for (var i = 0; i < Polygon.Length; i++)
            {
                var p = Polygon[i];
                minX = Math.Min(minX, p.x);
                minY = Math.Min(minY, p.y);
                maxX = Math.Max(maxX, p.x);
                maxY = Math.Max(maxY, p.y);
            }

            Start = new Vector2Int(minX, minY);
            _nodes = new List<LinkNode>();
            var n = new int[maxX - minX, maxY - minY];
            for (var i = 0; i < maxX - minX; i++)
            for (var j = 0; j < maxY - minY; j++)
            {
                if (!navigable.ContainsKey(i + minX)) continue;
                if (!navigable[i + minX].ContainsKey(j + minY)) continue;
                n[i, j] = navigable[i + minX][j + minY];
            }

            _grid = new JPSGrid(n);
        }

        public bool Contains(Vector2Int position)
        {
            return UnityTools.IsPointInPolygon(Polygon, position);
        }

        public void Tailor(ref Vector2Int point)
        {
            point.x -= Start.x;
            point.y -= Start.y;
        }

        public void Restore(ref List<Vector2Int> result)
        {
            for (var i = 0; i < result.Count; i++)
            {
                var tmp = result[i];
                tmp.x += Start.x;
                tmp.y += Start.y;
                result[i] = tmp;
            }
        }
    }
}